/*
 * Environment:
 * Microsoft Visual Studio 2008 Version 9.0.21022.8 RTM
 * Microsoft (R) 32-bit C/C++ Optimizing Compiler Version 15.00.21022.08 for 80x86
 *
 * Authors: Sergey Zamyslov, Sergey Moshkov
 */

#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <time.h>

#define DEBUG 0

double function(double x) {
    return sin(x);
}

void initializeKnotsAndNodes(double a, double b,
                             double* knots, int knotCount,
                             double* nodesX, double* nodesY)
{
    int    i;
    double step = (b - a) / (knotCount - 1);

    double d = step * 0.9;
    double dOver2 = d / 2.0;

    knots[0]             = a;
    knots[knotCount - 1] = b;

    nodesX[0] = knots[0];
    nodesY[0] = function(nodesX[0]);

    srand((int)time(NULL));

    for (i = 1; i < knotCount - 1; i++)
    {
        knots[i] = i * step;
        knots[i] += (((double)rand() / RAND_MAX) * d) - dOver2;

        nodesX[i] = (knots[i] + knots[i - 1]) / 2;
        nodesY[i] = function(nodesX[i]);
    }

    i = knotCount - 1;
    nodesX[i] = (knots[i] + knots[i - 1]) / 2;
    nodesY[i] = function(nodesX[i]);

    nodesX[knotCount] = knots[knotCount - 1];
    nodesY[knotCount] = function(nodesX[knotCount]);
}

void printExtra(double* data, int n, int extra)
{
    char* format      = "%11.4f";
    char* extraFormat = "%24.18f\n";

    int i;

    if(n > 20)
    {
        return;
    }

    if(extra)
    {
        format = extraFormat;
    }

    for (i = 0; i < n; i++)
    {
        printf(format, data[i]);
    }
}

void print(double* data, int n)
{
    printExtra(data, n, 0);
}

// Simple random initializer
void initializeData(int equationCount,
                    double* subDiagonal, double* mainDiagonal, double* supDiagonal,
                    double* b,
                    int quaterMax)
{
    int    i;
    srand((int)time(NULL));

    for (i = 0; i < equationCount - 1; i++)
    {
        subDiagonal[i]  = ((double)rand() / RAND_MAX) * quaterMax;
        supDiagonal[i]  = ((double)rand() / RAND_MAX) * quaterMax;
        mainDiagonal[i] = ((double)rand() / RAND_MAX) * 2 * quaterMax + (quaterMax * 2);
        b[i]            = ((double)rand() / RAND_MAX) * 4 * quaterMax;
    }
    mainDiagonal[i]     = ((double)rand() / RAND_MAX) * 2 * quaterMax + (quaterMax * 2);
    b[i]                = ((double)rand() / RAND_MAX) * 4 * quaterMax;
}

void solveTDM(int equationCount,
              double* subDiagonal, double* mainDiagonal, double* supDiagonal,
              double* b,
              double* x)
{
    // Initialization
    int     i;
    size_t	sizeInBytes      = sizeof(double) * equationCount;
    double* mainDiagonalWork = (double*)malloc(sizeInBytes),
          * bWork            = (double*)malloc(sizeInBytes);

	mainDiagonalWork[0] = mainDiagonal[0];
	bWork[0]            = b[0];

    // Forward
    for (i = 1; i < equationCount; i++)
    {
        double m = subDiagonal[i-1] / mainDiagonalWork[i-1];

        mainDiagonalWork[i] = mainDiagonal[i] - m * supDiagonal[i-1];
        bWork[i]            = b[i] - m * bWork[i-1];
	}

    // Backward
    x[equationCount - 1] = bWork[equationCount - 1] / mainDiagonalWork[equationCount - 1];	
    for (i = equationCount - 2; i >= 0; i--)
    {
        x[i] = (bWork[i] - supDiagonal[i] * x[i + 1]) / mainDiagonalWork[i];
    }

    free(mainDiagonalWork);
    free(bWork);
}

double* clone(double* from, int size)
{
    int i;
    double* clone = (double*)malloc(sizeof(double) * size);

    for(i = 0; i < size; ++i)
    {
        clone[i] = from[i];
    }

    return clone;
}

double solveTDM_Block(int equationCount,
                    double* subDiagonal, double* mainDiagonal, double* supDiagonal,
                    double* b,
                    int blockCount,
                    double* x)
{
    size_t dataElementSize = sizeof(double);

    int blockRowCount = equationCount / blockCount;

    int blockIndex;
    int blockStartRow, blockEndRow;
    int blockRowIndex;

    double factor;

    double* auxiliaryMatrix[5];

    /* Timing */
	clock_t start, finish;
	double time;

    // Copy all data.
    subDiagonal = clone(subDiagonal, equationCount - 1);
    mainDiagonal = clone(mainDiagonal, equationCount);
    supDiagonal = clone(supDiagonal, equationCount - 1);
    b = clone(b, equationCount);

    start = clock();

    /*
     * Step one.
     */
    if(DEBUG)
    {
        printf("**********************************\n");
        printf("Step one.\n\n");
    }

    for(blockIndex = 0; blockIndex < blockCount; ++blockIndex)
    {
        if(DEBUG)
        {
            printf("Block #%d is here.\n", blockIndex);
        }

        blockStartRow = blockIndex * blockRowCount;
        blockEndRow = blockStartRow + blockRowCount - 1;

        for(blockRowIndex = blockStartRow; blockRowIndex < blockEndRow; ++blockRowIndex)
        {
            if(DEBUG)
            {
                printf("Processing row %d.\n", blockRowIndex);
            }

            factor = subDiagonal[blockRowIndex] / mainDiagonal[blockRowIndex];

            if(blockIndex > 0)
            {
                // Non-diagonal elements.
                subDiagonal[blockRowIndex] = -subDiagonal[blockRowIndex - 1] * factor;
            }
            else
            {
                subDiagonal[blockRowIndex] = 0.0;
            }

            mainDiagonal[blockRowIndex + 1] -= supDiagonal[blockRowIndex] * factor;
            b[blockRowIndex + 1] -= b[blockRowIndex] * factor;
        }

        if(DEBUG)
        {
            if(blockIndex > 0)
            {
                printf("Left non-diagonal elements:\n");
                print(&subDiagonal[blockStartRow], blockRowCount - 1);
            }

            printf("\n\n");
        }
    }

    // TODO: Sync here.

    /*
     * Step two.
     */
    if(DEBUG)
    {
        printf("**********************************\n");
        printf("Step two.\n\n");
    }

    for(blockIndex = 0; blockIndex < blockCount; ++blockIndex)
    {
        if(DEBUG)
        {
            printf("Block #%d is here.\n", blockIndex);
        }

        blockStartRow = blockIndex * blockRowCount;
        blockEndRow = blockStartRow + blockRowCount - 1;
        blockStartRow = max(blockStartRow, 1);

        // Backward.
        for(blockRowIndex = blockEndRow - 1; blockRowIndex >= blockStartRow; --blockRowIndex)
        {
            if(DEBUG)
            {
                printf("Processing row %d.\n", blockRowIndex);
            }

            factor = supDiagonal[blockRowIndex - 1] / mainDiagonal[blockRowIndex];

            if(blockIndex > 0)
            {
                if(blockRowIndex > blockStartRow)
                {
                    subDiagonal[blockRowIndex - 2] -= subDiagonal[blockRowIndex - 1] * factor;
                }
                else
                {
                    // If we edit element out of block, we take element from main diagonal.
                    mainDiagonal[blockRowIndex - 1] -= subDiagonal[blockRowIndex - 1] * factor;
                }
            }

            // Non-diagonal elements.
            supDiagonal[blockRowIndex - 1] = -supDiagonal[blockRowIndex] * factor;

            b[blockRowIndex - 1] -= b[blockRowIndex] * factor;
        }

        if(DEBUG)
        {
            printf("\n\n");
        }
    }

    // Auxiliary matrix.
    auxiliaryMatrix[0] = (double*)malloc(dataElementSize * (blockCount - 1));
    auxiliaryMatrix[1] = (double*)malloc(dataElementSize * blockCount);
    auxiliaryMatrix[2] = (double*)malloc(dataElementSize * (blockCount - 1));
    auxiliaryMatrix[3] = (double*)malloc(dataElementSize * blockCount);

    for(blockIndex = 0; blockIndex < blockCount; ++blockIndex)
    {
        blockStartRow = blockIndex * blockRowCount;
        blockEndRow = blockStartRow + blockRowCount - 1;

        if(blockIndex > 0)
        {
            auxiliaryMatrix[0][blockIndex - 1] = subDiagonal[blockEndRow - 1];
        }

        if(blockIndex < blockCount - 1)
        {
            auxiliaryMatrix[2][blockIndex] = supDiagonal[blockEndRow];
        }

        auxiliaryMatrix[1][blockIndex] = mainDiagonal[blockEndRow];
        auxiliaryMatrix[3][blockIndex] = b[blockEndRow];
    }

    auxiliaryMatrix[4] = (double*)malloc(dataElementSize * blockCount);

    solveTDM(blockCount,
        auxiliaryMatrix[0], auxiliaryMatrix[1], auxiliaryMatrix[2],
        auxiliaryMatrix[3],
        auxiliaryMatrix[4]);

    if(DEBUG)
    {
        printf("X:\n");
        print(auxiliaryMatrix[4], blockCount);
        printf("\n\n\n");

        for(blockRowIndex = 0; blockRowIndex < equationCount; ++blockRowIndex)
        {
            x[blockRowIndex] = 0.0;
        }
    }

    for(blockIndex = 0; blockIndex < blockCount; ++blockIndex)
    {
        blockStartRow = blockIndex * blockRowCount;
        blockEndRow = blockStartRow + blockRowCount - 1;

        x[blockEndRow] = auxiliaryMatrix[4][blockIndex];
    }

    if(DEBUG)
    {
        printf("X:\n");
        print(x, equationCount);
        printf("\n\n\n");
    }

    free(auxiliaryMatrix[0]);
    free(auxiliaryMatrix[1]);
    free(auxiliaryMatrix[2]);
    free(auxiliaryMatrix[3]);
    free(auxiliaryMatrix[4]);

    for(blockIndex = 0; blockIndex < blockCount; ++blockIndex)
    {
        blockStartRow = blockIndex * blockRowCount;
        blockEndRow = blockStartRow + blockRowCount - 1;

        for(blockRowIndex = blockStartRow; blockRowIndex < blockEndRow; ++blockRowIndex)
        {
            x[blockRowIndex] = b[blockRowIndex];

            if(blockIndex > 0)
            {
                x[blockRowIndex] -= subDiagonal[blockRowIndex - 1] * x[blockStartRow - 1];
            }

            x[blockRowIndex] -= supDiagonal[blockRowIndex] * x[blockEndRow];

            x[blockRowIndex] /= mainDiagonal[blockRowIndex];
        }
    }

    finish = clock();
    time = ((double)finish - (double)start) / CLOCKS_PER_SEC;

    free(subDiagonal);
    free(mainDiagonal);
    free(supDiagonal);
    free(b);

    return time;
}

void calculateResiduals(int equationCount,
                        double* subDiagonal, double* mainDiagonal, double* supDiagonal,
                        double* b,
                        double* x,
                        double* residuals)
{
    int i;

    // First row.
    i = 0;
    residuals[i] = mainDiagonal[i] * x[i];
    residuals[i] += supDiagonal[i] * x[i + 1];
    residuals[i] -= b[i];

    for(i = 1; i < equationCount - 1; ++i)
    {
        residuals[i] = mainDiagonal[i] * x[i];

        residuals[i] += subDiagonal[i - 1] * x[i - 1];
        residuals[i] += supDiagonal[i] * x[i + 1];

        residuals[i] -= b[i];
    }

    // Last row.
    i = equationCount - 1;
    residuals[i] = mainDiagonal[i] * x[i];
    residuals[i] += subDiagonal[i - 1] * x[i - 1];
    residuals[i] -= b[i];
}

double getSplinePointY(int knotCount, double* knots,
                       double* nodesX, double* nodesY,
                       double* coeffs,
                       double x)
{
    int i;
    double c;

    for (i = 1; i < knotCount; ++i)
    {
        if (x <= knots[i])
        {
            break;
        }
    }

    if (i < knotCount)
    {
        c = x - nodesX[i];

        return nodesY[i] +
            0.5 * (coeffs[i] + coeffs[i - 1]) * c + 
            (coeffs[i] - coeffs[i - 1]) * c * c / (2 * (knots[i] - knots[i - 1]));
    }

    return 0.0;
}

void solveSubbotinCoeffs(double* knots, int knotCount, double* nodesY,
                         int blockCount, double* coeffs)
{
    double* subDiagonal  = (double*)malloc((knotCount - 1) * sizeof(double)),
          * supDiagonal  = (double*)malloc((knotCount - 1) * sizeof(double)),
          * mainDiagonal = (double*)malloc(knotCount * sizeof(double)),
          * b            = (double*)malloc(knotCount * sizeof(double));

    int i;
    double time;

    for (i = 1; i < knotCount - 1; i++)
    {
        subDiagonal[i - 1] = knots[i] - knots[i - 1];
        supDiagonal[i - 1] = knots[i] - knots[i - 1];
        mainDiagonal[i]    = 3 * (knots[i + 1] - knots[i - 1]);
        b[i - 1]           = 8 * (nodesY[i] - nodesY[i - 1]);
    }

    mainDiagonal[0] = 3 * (knots[1] - knots[0]);
    mainDiagonal[knotCount - 1] = 3 * (knots[knotCount - 1] - knots[knotCount - 2]);
    subDiagonal[knotCount - 2]  = knots[knotCount - 1] - knots[knotCount - 2];
    supDiagonal[knotCount - 2]  = knots[knotCount - 1] - knots[knotCount - 2];
    b[knotCount - 2]            = 8 * (nodesY[knotCount - 1] - nodesY[knotCount - 2]);
    b[knotCount - 1]            = 8 * (nodesY[knotCount] - nodesY[knotCount - 1]);

    time = solveTDM_Block(knotCount, subDiagonal, mainDiagonal, supDiagonal, b, blockCount, coeffs);
    printf("Time: %f\n\n\n", time);

    free(subDiagonal);
    free(supDiagonal);
    free(mainDiagonal);
    free(b);
}

int main(int argc, char* argv[])
{
    double  intervalStart = 0.0;
    double  intervalEnd = 10.0;
    int     knotCount   = 5;
    double* knots      = (double*)malloc(knotCount * sizeof(double));
    double* coeffs     = (double*)malloc(knotCount * sizeof(double));
    double* nodesX     = (double*)malloc((knotCount + 1) * sizeof(double));
    double* nodesY     = (double*)malloc((knotCount + 1) * sizeof(double));

    FILE* outputFile;
    char* outputFileName = "chart.csv";
    int i;
    double chartDx = 0.1;
    double chartX;

	// Initialization
    int     equationCount  = 12,
            blockCount     = 1,
            quaterInterval = 8000;

    size_t  sizeInBytes    = sizeof(double) * equationCount;
    double* subDiagonal   = (double*)malloc(sizeInBytes - sizeof(double)),
          * mainDiagonal  = (double*)malloc(sizeInBytes),
          * supDiagonal   = (double*)malloc(sizeInBytes - sizeof(double)),
          * b             = (double*)malloc(sizeInBytes),
          * x             = (double*)malloc(sizeInBytes),
          * residual      = (double*)malloc(sizeInBytes);

    // Data.
    initializeData(equationCount,
                   subDiagonal, mainDiagonal, supDiagonal,
                   b,
                   quaterInterval);

    printf("Set of equations:\n");
    printf("subDiagonal: ");
    print(subDiagonal, equationCount - 1);
    printf("\n");
    printf("mainDiagonal:");
    print(mainDiagonal, equationCount);
    printf("\n");
    printf("supDiagonal: ");
    print(supDiagonal, equationCount - 1);
    printf("\n");
    printf("b:           ");
    print(b, equationCount);
    printf("\n");



    // Test non-block
    printf("\n\n\nSerial implementation.\n");
    solveTDM(equationCount,
             subDiagonal, mainDiagonal, supDiagonal,
             b, 
             x);

    // Test residual
    calculateResiduals(equationCount,
        subDiagonal, mainDiagonal, supDiagonal,
        b,
        x,
        residual);

    // Print results
    printf("X:           ");
    print(x, equationCount);
    printf("\n");
    printf("residual:\n");
    printExtra(residual, equationCount, 1);
    printf("\n");



    // Block implementation.
    printf("\n\n\nBlock implementation.\n");
    solveTDM_Block(equationCount,
        subDiagonal, mainDiagonal, supDiagonal,
        b,
        blockCount,
        x);

    calculateResiduals(equationCount,
        subDiagonal, mainDiagonal, supDiagonal,
        b,
        x,
        residual);

    printf("X:           ");
    print(x, equationCount);
    printf("\n");
    printf("residuals:\n");
    printExtra(residual, equationCount, 1);
    printf("\n");

    free(subDiagonal);
    free(mainDiagonal);
    free(supDiagonal);
    free(b);
    free(x);
    free(residual);

    // test for subbotin coeffs
    initializeKnotsAndNodes(intervalStart, intervalEnd, knots, knotCount, nodesX, nodesY);
    printf("Knots:\n");
    print(knots, knotCount);
    printf("\n\n");

    printf("Nodes:\n");
    print(nodesX, knotCount + 1);
    printf("\n\n");

    solveSubbotinCoeffs(knots, knotCount, nodesY, blockCount, coeffs);
    print(coeffs, knotCount);
    printf("\n");

    outputFile = fopen(outputFileName, "w+");

    fprintf(outputFile, "X;Y;splineY;nodesY\n");
    for(chartX = intervalStart; chartX < intervalEnd; chartX += chartDx) {
        fprintf(outputFile, "%f;%f;%f;NA\n",
            chartX,
            function(chartX),
            getSplinePointY(knotCount, knots, nodesX, nodesY, coeffs, chartX));
    }

    chartX = intervalEnd;
    fprintf(outputFile, "%f;%f;%f;NA\n", chartX,
        function(chartX),
        getSplinePointY(knotCount, knots, nodesX, nodesY, coeffs, chartX));

    for(i = 0; i <= knotCount; ++i) {
        fprintf(outputFile, "%f;NA;NA;%f\n",
            nodesX[i],
            nodesY[i]);
    }

    fclose(outputFile);

    free(knots);
    free(coeffs);
    free(nodesX);
    free(nodesY);

    return 0;
}
